#include <ros/ros.h>
#include <ubt_core_msgs/JointCommand.h>
#include "kinematic.h"

int main(int argc, char **argv) {
    Matrix_Init();
    ros::init(argc, argv, "Kinematic_node");
    ros::NodeHandle nh;

    ros::Publisher left_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftLimb/controller", 10);
    ros::Publisher right_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightLimb/controller", 10);
    
    ubt_core_msgs::JointCommand left_arm_data;
    left_arm_data.command.resize(7);
    left_arm_data.mode = 5;

    ubt_core_msgs::JointCommand right_arm_data;
    right_arm_data.command.resize(7);
    right_arm_data.mode = 5;

    Eigen::VectorXd left_joints(7);
    left_joints << 0,0,0,0,0,0,0;

    Eigen::VectorXd right_joints(7);
    right_joints << 0,0,0,0,0,0,0;

    Eigen::Vector3d position, orientation;

    ros::Rate loop_rate(1000);
    double time_count = 0;
    while(ros::ok())
    {
        time_count += 0.003;
        position << 400 + 50*sin(M_PI*2*time_count), 200, 200 + 50*cos(M_PI*2*time_count);
        orientation << 0, M_PI/2, -M_PI/2;
        if(leftArmInverse(position, orientation, left_joints) == true)
        {
            left_arm_data.command[0] = left_joints(0);
            left_arm_data.command[1] = left_joints(1);
            left_arm_data.command[2] = left_joints(2);
            left_arm_data.command[3] = left_joints(3);
            left_arm_data.command[4] = left_joints(4);
            left_arm_data.command[5] = left_joints(5);
            left_arm_data.command[6] = left_joints(6);
            left_arm_pub.publish(left_arm_data);
        }

        position << -400 - 50*sin(M_PI*2*time_count), 200, 200 + 50*cos(M_PI*2*time_count);
        orientation << 0, M_PI/2, 0;
        if(rightArmInverse(position, orientation, right_joints) == true)
        {
            right_arm_data.command[0] = right_joints(0);
            right_arm_data.command[1] = right_joints(1);
            right_arm_data.command[2] = right_joints(2);
            right_arm_data.command[3] = right_joints(3);
            right_arm_data.command[4] = right_joints(4);
            right_arm_data.command[5] = right_joints(5);
            right_arm_data.command[6] = right_joints(6);
            right_arm_pub.publish(right_arm_data);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}